// VERSION: 9.July 2010
// COMMENTS: - this version has newtonian dynamics using Verlet integration, tested only on the simulator.
//           - refraction simulation works fine

#include "LivingSpot.h"
#include "hardwareIO.h" // rem: an extern SPI object is declared here, to be used both by the main program and the LivingSpot class

// modifs for next version: 
// (1) put thresholds here (global variables, easily changeable)
// (2) put limits of mirror excursion here too, easily changeable
// (3) add mode test without mirrors, using processing code

// ATTN: SRAM is very limited here (8kBytes), so the number of spots we can instantiate is quite limited (max 5 or 6?). 
#define NBSPOT 4 
LivingSpot spotArray[NBSPOT];
LivingSpot textSpot; // this is a special spot for TEXT DISPLAY (make a new class!)

#define NUM_PREDEF_MODES 10 // (see initModes.pde)
int predefinedMode=1;

// variables for scheduler: 
unsigned long schedulerModeTime;
unsigned long schedulerTime;
int schedulerMode;
byte lastNote =128;

int counterMonocyle=0;

boolean sendSerial=false; // this is for tests without the mirrors using the mirrorSimulator.pde on processing. 

void setup() {

	IO.init();  // set all the IO functions (PWM generation, SPI initialization, etc).
        // All other IO methods can be called AFTER we called the IO.init
 
        // Set interrupts for button control (this is a hack for setting an interrupt on a class member method):
	attachInterrupt(INTERRUPT_READ_BUTTONS, readButtonGlobal, RISING); 
 
        //Lockin resolution: 
        IO.setAnalogReadMode(EIGHT_BITS); // for lock-in readout mode (can be TEN or EIGHT bits, the last being a little faster). 
        
	// Show laser powers at init (regardless of their init value defined in the spot.init method (see schedule init):
	IO.setRedPower_mW(MAX_RedLaserPower);  
	IO.setGreenPower(HIGH);

	// Set Mirrors at init at their center position:
	IO.setMirrorsCenter();

	// Display splash screen: 
	IO.clearLCD();
	IO.displayLineLCD(" SMART LASER    ",1);
	IO.displayLineLCD(" PROJECTOR      ",2);

	// bips: 
	IO.buzzerBip(10, 3);

	// Initialize MIDI: 
	midiInit();

	// Setup serial communication with the computer (this will be useful for a real GUI, as well as for tests, scans, etc). 
	Serial.begin(115200);
	//sendSerial=true;

      // Instantiation and default initialization of the spots =================================================================
      // (ATTN: since we are not using an overloaded constructor, but an init function for the spots, we cannot
      // properly use spot methods before their "initialization" 
      
      for(int i=0; i<NBSPOT; i++){
		//Init parameters: (int posX, int posY, int saccadeRadius, int numSample, int id)
		spotArray[i].Init(CENTER_AD_MIRROR_X,  CENTER_AD_MIRROR_Y, 15 , 30, i);
                float mirrorSpanX=spotArray[i].xMax-spotArray[i].xMin;
                float mirrorSpanY=spotArray[i].yMax-spotArray[i].yMin;
                spotArray[i].updatePosition(spotArray[i].xMin+1.0*(i+1)*mirrorSpanX/(NBSPOT+1), CENTER_AD_MIRROR_Y);
                spotArray[i].alive=false; // default: inactive
      }
      
      // initialization of the text spot:
      textSpot.Init(CENTER_AD_MIRROR_X,  CENTER_AD_MIRROR_Y, 15 , 30, -1); // identifier of text spot is -1.


      // CALIBRATION (done on spot[0], but copy parameters for all the other spots): 
      // (note: we can relaunch a calibration by pressing the front panel button (upper-left):
        spotArray[0].RangeCalibration();
       spotArray[0].RangeCalibration(); // I do it two times, because somthing seems wrong the first time... 
       // (TO CHECK! some parameters are NOT initialized properly, and are initialized in the RangeCalibration routine?)
      // also, copy the calibration to all the other objects (alive or not): 
        copyCalibration();
                        
/*
      IO.setGreenPower(HIGH);
      IO.buzzerBip(30, 1);
      for (int i=0; i<20; i++) spotArray[0].computeNormalVector(2000, 2000, 50*i); // HERE: call something else that is not computeNormalVector, it can be
      // the scanTrajectory, but we need to pass parameters; instead, we can do a starDrawing(size) function for instance, that generates a "star" and "wraps" scanTrajectore... 
      for (int i=20; i>0; i--) spotArray[0].computeNormalVector(2000, 2000, 50*i);
      IO.setGreenPower(LOW);
 */     

      // Initialization of modes (activation and setting) ========================================================================
      // (A) EITHER USE A SCHEDULER: 
      schedulerInit();
      // (B) Or initialize here a default mode: 
      //predefinedMode=0;
      //modeChange(predefinedMode);
      
      // For tests: bouncing letters:
     //simpleBouncingOnly(3);
     //spotArray[0].set_circularSaccadeFromChar('P');
     // spotArray[0].setAperSaccade(100); 
     // spotArray[1].set_circularSaccadeFromChar('L');
     // spotArray[2].set_circularSaccadeFromChar('P');
     
}

//==========================  LOOP ================================================================

void loop(){

  // FIRST, check if we need to change the MODE OF OPERATION:
  scheduler();
  
  // for tests: 
  //IO.displayLineLCD(spotArray[0].auto_Threshold_Volts,2);	
  //IO.displayLineLCD(spotArray[0].depth_Range,2);	
  
  // char test[9]="SIGGRAPH";
  // textSpot.drawTrajectoryFromString(test, 8, true, 0, true, 2000,2000, 40, 0);
                
     
	// Process button data (rem: we could do this on the interrupt callback function, but then we cannot use serial nor delay functions; it is 
	// perhaps better to just do continuous polling on a single boolean variable called IO.processButton:
	if (IO.processButton) {
		buttonProcessing();// process button commands
		IO.processButton=false;// stop servicing buttons until we press again any button 
	} // end of button processing. 

         // ***FOR TESTS ONLY (better not to leave this here, it's time consuming and if someone touches the potentiometer, the delay angle can be modified by error: 
        /*
        IO.readPotB(); // normalized value in valuePotB (also return of this function)
        spotArray[0].error_angle=2*PI*(IO.valuePotB-0.5); // change correction angle (radians)
         // copy on all spots: 
         for (int i=1; i<NBSPOT; i++) spotArray[i].error_angle=spotArray[0].error_angle;
        */
         // ******************

        // UPDATE SPOT DYNAMICS AND SOUND:
        // REM: sound update should be done in the "update" method, as a function of the mode (update method is in LivingSpot.cpp), but the pb is that the midi functions are not visible
        // there (it's not a .pde). So, we will use instead a global midiSoundGeneration() function that takes a several spot parameters (this would be equivalent to the OSC send data!)
	// Go over all the spots that are alive (call their update functions), and process events (collisions, send data, etc). 
	for(int i=0; i<NBSPOT; i++){
		if(spotArray[i].alive){
  
                // **************** First, the BEHAVIOUR update 
	        spotArray[i].update(spotArray, NBSPOT);

                // **************** Other visual effects and decorations (more for SLP than for scorelight): 
                 
                 // Display distance: 
                 if ( spotArray[i].displayDistance ) { 
                   //char test[9]="SIGGRAPH";
                  char printstring[9];
                  // convert to string: (use function dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER) in #include<stdlib.h>) 
	           dtostrf(spotArray[i].depth_Range,1,0,printstring);
                   textSpot.drawTrajectoryFromString(printstring, true, MAX_RedLaserPower, true, spotArray[i].getAz()+4*spotArray[i].nx*textSpot.Aper_DA, spotArray[i].getEl()+4*spotArray[i].ny*textSpot.Aper_DA, 40, 0);
                 }
                 // Display Angles
                  if ( spotArray[i].displayAngle ) { 
                    float darkProportion=1-spotArray[i].whiteProportion;
                    if ((darkProportion>.125)&&(darkProportion<0.33)) {
                   //char test[9]="SIGGRAPH";
                  char printstring[9];
                  // convert to string: (use function dtostrf(FLOAT,WIDTH,PRECSISION,BUFFER) in #include<stdlib.h>) 
	           dtostrf(360*darkProportion,1,0,printstring);
                  float angleRencentering=atan2(spotArray[i].nx,-spotArray[i].ny);
                    for (int j=0; j<10 ; j++) textSpot.drawTrajectoryFromString(printstring, true, MAX_RedLaserPower, true, spotArray[i].getAz()+2.5*spotArray[i].nx*spotArray[i].Aper_DA, spotArray[i].getEl()+2.5*spotArray[i].ny*spotArray[i].Aper_DA, 40, angleRencentering);
                 }
                  }
                // Monocyle:
                if (spotArray[i].displayMonocycle ) {
                 char letterMonocyle;
                 counterMonocyle++;
                  if (counterMonocyle==10) letterMonocyle='b';
                  else if (counterMonocyle==20) letterMonocyle='a';
                  else if (counterMonocyle==30) {letterMonocyle='c'; counterMonocyle=0;}
                  float angleRencentering=atan2(spotArray[i].nx,-spotArray[i].ny);
                  textSpot.drawTrajectoryFromChar(letterMonocyle, MAX_RedLaserPower, HIGH, spotArray[i].getAz()+2.5*spotArray[i].nx*spotArray[i].Aper_DA, spotArray[i].getEl()+2.5*spotArray[i].ny*spotArray[i].Aper_DA, 2.5*spotArray[i].Aper_DA, angleRencentering);
                }


                // // **************** SOUND GENERATION: 
                // (a) Using free bass and super bass:  
                midiSoundGeneration(&spotArray[i]);
  
                // (b) other sounds (outdated?):  
		if(spotArray[i].touchWall) midiTouchWall(i);
		if(spotArray[i].collision) midiCollision(i, spotArray[i].target_angle);
		if(spotArray[i].behaviourMode == COUNTOUR_FOLLOWING) midiPitchAngle(i, spotArray[i].onContour, spotArray[i].target_angle);

		// Send OSC: 
	        // .. to do. 

		}    
	}


 // Process Serial commands: 
                        if (Serial.available()>0) processSerial();
			
                        // To put in processSerial (note: send data for one or more spots):   
                        // send coordinates of spot (regardless of spot number) to processing:  
			if (sendSerial==true) {
				delay(50); // this is just to slow down execution in case we use Processing to check data
				// Send data: 
				Serial.print(int(spotArray[0].getAz()), DEC);  // rem: getX gives a float between 0 and 4000
				Serial.print('X'); 
				Serial.print(int(spotArray[0].getEl()), DEC);
				Serial.print('Y'); 
				Serial.print(spotArray[0].identifier);
				Serial.print('I'); // for identifier (this will enable setting the coordinates for the proper spot
				// on the processing program).
			}

}



// HACK for setting an interrupt on a class member method:
void readButtonGlobal() {
	// NOTE: while inside this functions, interruptions are disabled
	IO.readButtons(); 
}


void buttonProcessing() {
  // Announce that we are entering button processing: 
		IO.buzzerBip();
		digitalWrite(34,HIGH);
		delay(100);
		digitalWrite(34,LOW);
		IO.clearLCD();
		//IO.displayLineLCD(IO.buttonByte, 1);


		// MODE BUTTON (LEFT, BOTTOM): just cycles through all the predefined modes:
		if (!((IO.buttonByte)&B10000000)) { // pin 22 (note: active button is 0)		
                      // change predefined mode (by cycling between all the predefined modes)
                      predefinedMode=(predefinedMode+1)%NUM_PREDEF_MODES;
                        IO.clearLCD();
                      IO.displayLineLCD("CHANGE MODE     ", 1);
                      // IO.displayLineLCD("         ", 2); // HERE, put the name of the mode (make a list).
                       IO.buzzerBip(300,3);
                       modeChange(predefinedMode);
		}

              // CALIBRATION
              //int RangeCalibration(void);  
      	      if (!((IO.buttonByte)&B01000000)) { // pin 28 (note: active button is 0)                  
                        spotArray[0].RangeCalibration();
                        // also, copy the calibration to all the other objects (alive or not): 
                        copyCalibration();
		}

              // CHECK DELAY (recentering vector display), controlled by right potentiometer (B)
              // REM: it should be a MODE of the spot???
              if (!((IO.buttonByte)&B00100000)) { // pin 27 (note: active button is 0)                  
                       
                        // Stay here until we press the "CANCEL" button (number 29) or better, the SAME button:
                         IO.clearLCD();
                         IO.displayLineLCD("SET ANGULAR     ", 1);
                         IO.displayLineLCD("   DELAY        ", 2);
			 IO.displayLineLCD(float(180.0/PI*spotArray[0].error_angle),2); 
                         IO.buzzerBip(300,3);
                         delay(1000);

                         IO.readButtons(); // do this in order to clear the buttonByte
                        while (((IO.buttonByte)&B00100000)) { // this is pin 27
                          IO.readPotB(); // normalized value in valuePotB (also return of this function)
                          spotArray[0].error_angle=2*PI*(IO.valuePotB-0.5); // change correction angle (radians)
                         // spotArray[0].checkRecenteringVector(spotArray[0].getAz(), spotArray[0].getEl(), spotArray[0].Aper_DA); // display recentering vector
                         spotArray[0].checkRecenteringVector(spotArray[0].getAz(), spotArray[0].getEl(), spotArray[0].Aper_DA); // display recentering vector
                        
                        
                        // show data in real time (not really necessary, we are seeing this with the laser):
                         IO.clearLCD();
                         IO.displayLineLCD("error angle:", 1);
			 IO.displayLineLCD(float(180.0/PI*spotArray[0].error_angle),2); 

                          IO.readButtons(); 
                        }
                        
                        // EXIT: show final value, and save modified values to other spots: 
                        for (int i=1; i<NBSPOT; i++) {
                          spotArray[i].error_angle=spotArray[0].error_angle;
                        }
                         
                         // final value:
                         IO.clearLCD();
                         IO.displayLineLCD("error angle:", 1);
			 IO.displayLineLCD(spotArray[0].error_angle,2); 
                         IO.buzzerBip(300,1);
                         delay(1000);
              }

               // Draw max area: 
              //void drawMaxArea(int nloops = 2); //draw a squarre corresponding to the maximum area
              
              
              // FREQUENCY CHANGE: 
		if (!((IO.buttonByte)&B00000100)) { // INCREMENT FREQUENCY (pin 24)
			IO.incRefFreq(1);
			IO.clearLCD();
			IO.displayLineLCD("REF FREQ (kHz)", 1);
			IO.displayLineLCD(IO.refFreq,2); 
		}

		if (!((IO.buttonByte)&B00001000)) { // DECREMENT FREQUENCY (pin 25)
			IO.decRefFreq(1);
			IO.clearLCD();
			IO.displayLineLCD("REF FREQ (kHz)", 1);
			IO.displayLineLCD(IO.refFreq,2); 
		} 

		// BUTTON/MENU TO CHANGE THE BEHAVIOUR: 
		// ...

		// ADD/DELETE POINTS:
		// ... 


		// Threshold values: FOR THE TIME BEING LET'S DO CONTINOUS POLLING ON THE POTENTIOMETER A:
                    IO.readPotA(); // normalized value in valuePotA (also return of this function). REM: min resolution =I_Volts_SAT/1024 ~ 0.005  
                 // modify both depth and fixed threshold:
                 float oldValue=1.0*spotArray[0].fixed_Threshold_Volts/I_Volts_SAT;
                 if ((oldValue<IO.valuePotA-0.005)||(oldValue>IO.valuePotA+0.005)) { // make a tolerance range to avoid too much update because of reading noise:
                  spotArray[0].fixed_Threshold_Volts=IO.valuePotA*I_Volts_SAT; // 0 to 5V
                  spotArray[0].updateDepthThreshold(500.0*IO.valuePotA);// 0 to 500 cm...
                 // update values for all the spots (optional):
	        for(int i=1; i<NBSPOT; i++){
                    spotArray[i].fixed_Threshold_Volts=spotArray[0].fixed_Threshold_Volts;
                    spotArray[i].depth_Threshold_Volts=spotArray[0].depth_Threshold_Volts;
                 }
                 // show new values:
                    IO.clearLCD();
			  //IO.displayLineLCD("depth threshold:", 1);
			  IO.displayLineLCD(spotArray[0].depth_Threshold_Volts,1); 
                          // IO.displayLineLCD("fixed volt thre:", 1);
			  IO.displayLineLCD(spotArray[0 ].fixed_Threshold_Volts,2); 
                          IO.readButtons(); 
                 }
/*
		if (!((IO.buttonByte)&B00010000)) { // pin 26 (potA)
                        IO.clearLCD();
			IO.displayLineLCD("SET THRESHOLD   ", 1);
                        IO.displayLineLCD("(LEFT POT)      ", 2);
			 IO.buzzerBip(300,3);
                         delay(1000);

                            IO.readButtons(); // do this in order to clear the buttonByte
                         while (((IO.buttonByte)&B00010000)) { // this is pin 26
                         IO.readPotA(); // normalized value in valuePotA (also return of this function)
                         
                          // modify both depth and fixed threshold:
                          spotArray[0].fixed_Threshold_Volts=IO.valuePotA*I_Volts_SAT; // 0 to 5V
                          spotArray[0].updateDepthThreshold(500.0*IO.valuePotA);// 0 to 500 cm...
                          
                          spotArray[0].checkRecenteringVector(spotArray[0].getAz(), spotArray[0].getEl(), spotArray[0].Aper_DA); // display recentering vector
                        
                          //show modified values:
                          IO.clearLCD();
			  //IO.displayLineLCD("depth threshold:", 1);
			  IO.displayLineLCD(spotArray[0].depth_Threshold_Volts,1); 
                          // IO.displayLineLCD("fixed volt thre:", 1);
			  IO.displayLineLCD(spotArray[0 ].fixed_Threshold_Volts,2); 
                          IO.readButtons(); 
                        }

			// update values for all the spots (optional):
			for(int i=1; i<NBSPOT; i++){
                          spotArray[i].fixed_Threshold_Volts=spotArray[0].fixed_Threshold_Volts;
                          spotArray[i].depth_Threshold_Volts=spotArray[0].depth_Threshold_Volts;
                        }
			
                        // final values:
                        IO.clearLCD();
			IO.displayLineLCD("depth threshold:", 1);
			IO.displayLineLCD(spotArray[0].depth_Threshold_Volts,2); 
                         IO.buzzerBip(300,1);
                        delay(1000);
                        IO.displayLineLCD("fixed volt thre:", 1);
			IO.displayLineLCD(spotArray[0].fixed_Threshold_Volts,2); 
                        IO.buzzerBip(300,1);
                        delay(1000);
		}
*/

}


// =========== 
// SAVE/COPY calibration: 
// In the future, we will use a class with its in-build copy constructor to avoid doing it by hand for copying between one spot and another.
// The class will also provide a method to save/load from EEPROM. 
// ...TO DO

void copyCalibration() { // copy calibration parameters from spot 0
for (int i=1; i<NBSPOT; i++) {
spotArray[i].I_Volts_REF=spotArray[0].I_Volts_REF; 
spotArray[i].RedLaserPower_REF=spotArray[0].RedLaserPower_REF;
spotArray[i].I_Volts_OPTIMAL=spotArray[0].I_Volts_OPTIMAL;
spotArray[i].MIN_CONTRAST_Volts=spotArray[0].MIN_CONTRAST_Volts;
spotArray[i].Attenuation_Factor=spotArray[0].Attenuation_Factor;
spotArray[i].I_Volts_background=spotArray[0].I_Volts_background;
spotArray[i].sigm_I_Volts_background=spotArray[0].sigm_I_Volts_background;
spotArray[i].I_Volts_maxLaser=spotArray[0].I_Volts_maxLaser;
spotArray[i].I_Volts_minLaser=spotArray[0].I_Volts_minLaser;
spotArray[i].MAX_DEPTH_RANGE=spotArray[0].MAX_DEPTH_RANGE;
spotArray[i].MIN_DEPTH_RANGE=spotArray[0].MIN_DEPTH_RANGE;
spotArray[i].Proper_Calibration_Flag=spotArray[0].Proper_Calibration_Flag; 
spotArray[i].I_Volts_UpperMarge=spotArray[0].I_Volts_UpperMarge; 
spotArray[i].I_Volts_LowerMarge=spotArray[0].I_Volts_LowerMarge;
spotArray[i].Distance_Intensity_Model=spotArray[0].Distance_Intensity_Model;
// optional:
spotArray[i].fixed_Threshold_Volts=spotArray[0].fixed_Threshold_Volts; //(actually, what is optional in this case is to set this in the rangecalibration function)
spotArray[i].PowerCorrectionMode=spotArray[0].PowerCorrectionMode;
// RadiusCorrectionMode: better not to change this. 
}
}
	
